/********************************************************************************
 * Copyright(c) 2020-2025 BST All rights reserved.
 * - Filename
 * - Author
 * - Version V1.0.0
 * - Date    2024/08/28
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */
#include "ApolloLidarAdapter.h"

#include <sys/time.h>

#include "TimeUtil.h"

using namespace lidar;

ApolloLidarAdapter::ApolloLidarAdapter(const std::string &interface,
                                       const std::string &ip,
                                       const uint32_t &id)
    : _interface(interface), _ip(ip), _id(id) {
  std::string info;
  info.append("Interface:");
  info.append(interface);
  info.append("Id:");
  info.append(std::to_string(_id));
  info.append(", IP:");
  info.append(ip);
  std::cout << "ApolloLidarAdapter Impl create : " << info << std::endl;
}

ApolloLidarAdapter::~ApolloLidarAdapter() {
  std::cout << "ApolloLidarAdapter Destroy..." << std::endl;
}

void ApolloLidarAdapter::start(
    std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        callback) {
  _isExit = false;
  _lidar = std::make_shared<aeye::Sensor>(_interface, _ip, std::to_string(_id));
  _lidar->setOnPointCloudFrameCallback(std::bind(
      &ApolloLidarAdapter::pointsCallback, this, std::placeholders::_1));
  _lidar->connect();
  // _lidar->sendPointReturnMask(aeye::PointReturnFlags::CARTESIAN_COORDINATES |
  //                             aeye::PointReturnFlags::INTENSITY);
  _lidar->sendShotlistFileNumber(0);
  _lidar->start();
  _packageThread.reset(new std::thread(
      std::bind(&ApolloLidarAdapter::processLoop, this, callback)));
}

void ApolloLidarAdapter::stop() {
  _lidar->stop();
  _isExit = true;
  _condition.notify_all();
  if (_packageThread != nullptr && _packageThread->joinable()) {
    _packageThread->join();
  }
  _lidar->disconnect();
  std::cout << "ApolloLidarAdapter Exit ..." << std::endl;
}

void ApolloLidarAdapter::processLoop(
    const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        &callback) {
  while (!_isExit) {
    std::unique_lock<std::mutex> lck(_mtx);
    _condition.wait(lck,
                    [this] { return _isExit || !_lidarFrameDeque.empty(); });
    if (_isExit) break;
    auto lidarFrame = _lidarFrameDeque.front();
    _lidarFrameDeque.pop_front();
    lck.unlock();
    if (_isExit) break;

    if (TimeUtil::fps_calc_inc(_fps)) {
      std::cout << "[FPS]: lidar" << lidarFrame->lidar_id()
                << ", point count: " << lidarFrame->data_size()
                << " total fps:" << _fps.avg_fps << ", fps: " << _fps.ins_fps
                << std::endl;
    }
    callback(std::move(lidarFrame));
  }
}

void ApolloLidarAdapter::pointsCallback(aeye::PointCloudFrame &&originalFrame) {
  std::cout << "get frame: " << (int)originalFrame.getPoints().size()
            << std::endl;
  auto frame = std::make_shared<lpc::LidarProtoPointCloud>();
  frame->set_lidar_id(_id);
  frame->set_system_timestamp(TimeUtil::now_us());
  uint64_t timestamp = ((uint64_t)originalFrame.getTimeSec()) * 1000000 +
                       ((uint64_t)originalFrame.getTimeuSec());
  frame->set_timestamp(timestamp);  // us
  frame->set_seq(originalFrame.getFrameId());
  frame->set_height(originalFrame.getPoints().size());
  frame->set_width(1);
  frame->set_is_dense(false);

  for (int i = 0; i < (int)originalFrame.getPoints().size(); i++) {
    frame->add_data(originalFrame.getPoints()[i].pointReturnCartesianCoords.x);
    frame->add_data(originalFrame.getPoints()[i].pointReturnCartesianCoords.y);
    frame->add_data(originalFrame.getPoints()[i].pointReturnCartesianCoords.z);
    frame->add_data(((float)originalFrame.getPoints()[i]
                         .pointReturnIntensity.intensity_type_data) /
                    65535.0f * 255);
  }

  {
    std::unique_lock<std::mutex> lck(_mtx);
    _lidarFrameDeque.emplace_back(std::move(frame));
  }
  _condition.notify_one();
}
